

#include <iostream>

#include <Eigen/Dense>
#include <sophus/so3.hpp>

#define S double
using Vec3T = Eigen::Matrix<S, 3, 1>; 
using Mat3T = Eigen::Matrix<S, 3, 3>; 
using SO3 = Sophus::SO3<S>;
SO3 R_ = SO3::exp(Vec3T(0.1, 0.2, 0.3));

//解析方法
Vec3T ComputeYawJacobian() {
    Mat3T R_mat = R_.matrix();
    
    // 1. 计算偏航角对旋转矩阵的导数
    double R00 = R_mat(0, 0);
    double R10 = R_mat(1, 0);
    double denom = R00 * R00 + R10 * R10;
    
    if (denom < 1e-12) {
        // 奇异情况，返回默认值
        return Vec3T(0, 0, 1);
    }
    
    // ∂ψ/∂R 是一个1x9的行向量
    Eigen::Matrix<S, 1, 9> dpsi_dR = Eigen::Matrix<S, 1, 9>::Zero();
    dpsi_dR(0) = -R10 / denom;  // ∂ψ/∂R₀₀
    dpsi_dR(1) =  R00 / denom;  // ∂ψ/∂R₁₀
    
    // 2. 计算旋转矩阵对旋转误差的导数
    Eigen::Matrix<S, 9, 3> dR_ddeltaTheta = Eigen::Matrix<S, 9, 3>::Zero();
    
    // 对于每个δθ分量，计算其对R的影响
    for (int i = 0; i < 3; ++i) {
        // 创建反对称矩阵的基础
        Mat3T skew = Mat3T::Zero();
        int j = (i + 1) % 3;
        int k = (i + 2) % 3;
        skew(j, k) = -1;
        skew(k, j) =  1;
        
        // 关键修正：对于 R_true = R_nominal * Exp(δθ)
        // ∂R/∂δθ = R_nominal · G，没有负号！
        Mat3T dR_dtheta_i = R_mat * skew;
        
        // 将3x3矩阵展平为9x1向量
        dR_ddeltaTheta.col(i) = Eigen::Map<const Eigen::Matrix<S, 9, 1>>(dR_dtheta_i.data());
    }
    
    // 3. 链式法则：∂ψ/∂δθ = ∂ψ/∂R * ∂R/∂δθ
    Vec3T jacobian = dpsi_dR * dR_ddeltaTheta;
    
    return jacobian;
}

S GetYawFromSO3(const SO3& R) {
    Mat3T R_mat = R.matrix();
    // 从旋转矩阵提取偏航角 (ZYX欧拉角顺序)
    // ψ = atan2(R[1,0], R[0,0])
    double yaw = atan2(R_mat(1, 0), R_mat(0, 0)); // 提取偏航角
    
    // 确保角度在 [-π, π] 范围内
    if (yaw > M_PI) yaw -= 2 * M_PI;
    if (yaw < -M_PI) yaw += 2 * M_PI;
    
    return yaw;
}

//使用数值微分方法
Vec3T ComputeYawJacobianNew() {
    const double epsilon = 1e-8;
    Vec3T jacobian;
    
    double base_yaw = GetYawFromSO3(R_);
    
    // 对每个 δθ 分量进行扰动
    for (int i = 0; i < 3; ++i) {
        Vec3T delta_theta = Vec3T::Zero();
        delta_theta[i] = epsilon;
        
        // 计算扰动后的旋转矩阵
        // 关键：这里应该用左乘还是右乘？
        // 选项1：R_perturbed = R_ * SO3::exp(delta_theta);  // 右乘
        // 选项2：R_perturbed = SO3::exp(delta_theta) * R_;  // 左乘
        
        // 根据ESKF的标准定义，应该用右乘

        SO3 R_perturbed = R_ * SO3::exp(delta_theta);
        double perturbed_yaw = GetYawFromSO3(R_perturbed);
        
        // 数值微分
        jacobian[i] = (perturbed_yaw - base_yaw) / epsilon;
        
        // 角度归一化
        if (jacobian[i] > M_PI) jacobian[i] -= 2 * M_PI;
        if (jacobian[i] < -M_PI) jacobian[i] += 2 * M_PI;
    }
    
    return jacobian;
}

//中心差分实现
Vec3T ComputeYawJacobianCentered() {
    const double epsilon = 1e-8;
    Vec3T jacobian;
    
    for (int i = 0; i < 3; ++i) {

        // 关键：这里应该用左乘还是右乘？
        // 选项1：R_perturbed = R_ * SO3::exp(delta_theta);  // 右乘
        // 选项2：R_perturbed = SO3::exp(delta_theta) * R_;  // 左乘
        
        // 根据ESKF的标准定义，应该用右乘

        // 正扰动
        Vec3T delta_plus = Vec3T::Zero();
        delta_plus[i] = epsilon;
        SO3 R_plus = R_ * SO3::exp(delta_plus);
        double yaw_plus = GetYawFromSO3(R_plus);
        
        // 负扰动  
        Vec3T delta_minus = Vec3T::Zero();
        delta_minus[i] = -epsilon;
        SO3 R_minus = R_ * SO3::exp(delta_minus);
        double yaw_minus = GetYawFromSO3(R_minus);
        
        // 中心差分
        jacobian[i] = (yaw_plus - yaw_minus) / (2 * epsilon);
        
        // 角度归一化
        if (jacobian[i] > M_PI / epsilon) jacobian[i] -= 2 * M_PI / epsilon;
        if (jacobian[i] < -M_PI / epsilon) jacobian[i] += 2 * M_PI / epsilon;
    }
    
    return jacobian;
}

int main(int argc, char *argv[]) {

    /*
    //  在GNSS观测函数中：
        Vec3T yaw_jac = ComputeYawJacobian();
        H.template block<1, 3>(3, 6) = yaw_jac.transpose();  // 1x3行向量
    */
    Vec3T jacobian = ComputeYawJacobian();
    std::cout << "Yaw Jacobian: \n" << jacobian << std::endl;
    Vec3T jacobian_new = ComputeYawJacobianNew();
    std::cout << "Yaw Jacobian New: \n" << jacobian_new << std::endl;
    Vec3T jacobian_centered = ComputeYawJacobianCentered();
    std::cout << "Yaw Jacobian Centered: \n" << jacobian_centered << std::endl;

    Vec3T error = jacobian_centered - jacobian;
    std::cout << "Error: \n" << error << std::endl;

    double max_error = error.array().abs().maxCoeff();
    std::cout << "Max error:  " << max_error << std::endl;

    if (max_error > 1e-6) {
        std::cout << "Jacobian validation failed, max error: " << max_error << std::endl;
    }

    return 0;
}